#ifndef STEINER_ROUTING_H
#define STEINER_ROUTING_H


#include <set>
#include <list>
#include <vector>
#include <map>
#include <memory>
#include <algorithm>
#include <iostream>
#include "Point2D.h"
#include "SteinerPoint.h"
#include "MergePoint.h"

namespace STEINER {

template <typename T>
class Router
{
public:

    using MergePointSPtr = std::shared_ptr<MergePoint<T> >;
    using MergePointWPtr = std::weak_ptr<MergePoint<T> >;

    using SteinerPointSPtr = std::shared_ptr<SteinerPoint<T> >;
    using Point2DSPtr = std::shared_ptr<Point2D<T> >;

    Router() : length(0) {}
    ~Router() {}


    void SetPoints(const std::list<std::pair<T, T> >& coords) {
        points.clear();
        for (auto coord : coords) {
            points.emplace_back(std::make_shared<Point2D<T> >(coord.first, coord.second));
        }
        std::sort(points.begin(), points.end(), ComparePointSPtrOfK<T>);
        auto iter = std::unique(points.begin(), points.end());
        points.resize(std::distance(points.begin(), iter));
    }

    T GetRouterLength() {
        length = 0;
        size_t cnt = points.size() - 1;
        while (cnt >= 0) {
            Point2DSPtr bigPoint = points[cnt];
            std::set<SteinerPointSPtr, CompareSteinerPointK<T> > steiners = GetSteiners();
            if (steiners.empty()) {
                InsertPoint(bigPoint);
                --cnt;
                continue;
            }
            SteinerPointSPtr bigSteiner = *steiners.rbegin();
            if (static_cast<Point2D<T> >(bigSteiner) < static_cast<Point2D<T> >(bigPoint)) {
                InsertPoint(bigPoint);
                --cnt;
            } else {
                InsertSteinerPoint(bigSteiner);
            }
        }
        return length;
    }

    void UpdateLength(const MergePointSPtr& point) {
        length += point->GetLengthOfDominatedPoints();
    }

    void InsertPoint(const Point2DSPtr& point) {
        MergePointSPtr mergePt = std::make_shared<MergePoint<T> >(point->GetX(), point->GetY());
        GetDominatedPoints(point);
        UpdateLength(mergePt);
        RemoveMergePoints(mergePt);
        InsertMergePoint(mergePt);
    }

    void UpdateLength(const SteinerPointSPtr& point) {
        length += point->GetLengthOfParents();
    }

    void InsertSteinerPoint(const SteinerPointSPtr& point) {
        UpdateLength(point);
        std::list<MergePointWPtr> parents = point->GetParents();
        MergePointSPtr mergePt = std::make_shared<MergePoint<T> >(point->GetX(), point->GetY());
        for (const auto p : parents) {
            auto sp = p.lock();
            if (nullptr != sp) {
                BuildConn(sp, point);
                RemoveMergePoint(sp);
            }
        }
        InsertMergePoint(mergePt);
    }

    void InsertMergePoint(const MergePointSPtr& point) {
        point->CalAngelWithY();
        mergePoints.insert(point);
    }

    std::set<SteinerPointSPtr, CompareSteinerPointK<T> > GetSteiners() const {
        std::set<SteinerPointSPtr, CompareSteinerPointK<T> > ret;
        if (mergePoints.size()<= 1) {
            return ret;
        }
        if (2 == mergePoints.size()) {
            auto head = *mergePoints.begin();
            auto tail = mergePoints.back();
            SteinerPointSPtr steiner = SteinerPoint<T>::CalSteinerPoint(head, tail);
            if (nullptr != steiner) {
                ret.insert(steiner);
            }
            return ret;
        }

        for (auto it = mergePoints.begin(); it != mergePoints.end(); ++it) {
            auto current = it;
            auto next = std::next(it);
            if (next == mergePoints.end()) {
                current = mergePoints.begin();
            }
            SteinerPointSPtr steiner = SteinerPoint<T>::CalSteinerPoint(*current, *next);
            if (nullptr == steiner) {
                continue;
            }
            auto ifind = ret.find(steiner);
            if (ifind != ret.end()) {
                (*ifind)->AddParent(*current);
                (*ifind)->AddParent(*next);
            } else {
                ret.insert(steiner);
            }
        }
        return ret;
    }

    void RemoveDominatedPoint(const MergePointSPtr& point) {
        std::list<MergePointWPtr> dominatedPoints = point->GetDominatedPoints();
        for (const auto p : dominatedPoints) {
            auto sp = p.lock();
            if (nullptr != sp) {
                RemoveMergePoint(sp);
            }
        }
        point->ClearDominatedPoints();
    }

    void GetDominatedPoints(const MergePointSPtr& point) const {
        for (const auto p : mergePoints) {
            T x = GetMid(p->GetX(), point->GetX(), 0);
            T y = GetMid(p->GetY(), point->GetY(), 0);
            MergePointSPtr mid = std::make_shared<MergePoint<T> >(x, y);
            if (*mid == *point && *mid != *p) {
                BuildConn(p, point);
                point->AddDominatedPoint(p);
            }
        }
    }

    bool RemoveMergePoint(const MergePointSPtr& point) {
        auto it = mergePoints.find(point);
        if (mergePoints.end() != it) {
            mergePoints.erase(it);
            return true;
        }
        return false;
    }

    void RemoveMergePoints(const std::vector<MergePointSPtr>& points) {
        for (const auto& p : points) {
            RemoveMergePoint(p);
        }
    }

    void BuildConn(const MergePointSPtr& a, const MergePointSPtr& b) {
        a->AddConnPoint(b);
        b->AddConnPoint(a);
    }



protected:
    T length;
    std::vector<Point2DSPtr> points;
    std::list<MergePointSPtr> cache;
    std::set<MergePointSPtr, CompareMergeSPtrAngle<T> > mergePoints;
};

}

#endif
